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ABSTRACT 


With  the  Global  War  on  Terrorism  (GWOT)  taking  place  in  full  force, 
autonomous  vehicles  have  become  a  major  asset  to  government  forces.  Expansion  of 
single  vehicle  technology  to  multiple  vehicle  technology  is  required  in  order  for  the 
United  States  to  stay  ahead  of  its  adversaries  in  the  GWOT  and  other  technological  fields 
(such  as  oceanography).  Multiple  vehicle  technology  has  been  explored  by  many 
different  institutions  in  the  recent  past  (Leonard,  2001  and  Kucik,  2003).  Expansion  of 
this  technology  will  lead  to  greater  autonomy  and  robustness  amongst  the  vehicles.  This 
thesis  presents  a  simulation  of  a  “follow  the  leader”  behavior  for  multiple  Autonomous 
Underwater  Vehicles  (AUVs).  The  follower  vehicle  incorporates  the  use  of  forward- 
looking  sonar  to  track  the  leader  vehicle.  This  process  will  free  up  bandwidth  between 
acoustic  modems;  allowing  data  transfer  to  occur  with  greater  efficiency.  Hydrodynamic 
coefficients  are  used  to  develop  steering  equations  that  model  REMUS  through  a  track  of 
specified  waypoints  similar  to  a  real-world  mission  track.  A  two-dimensional  forward 
looking  sonar  model  with  a  120°  horizontal  scan  and  a  1 10  meter  radial  range  is  modeled 
to  track  the  leader  vehicle.  Resulting  bearing  and  range  between  the  two  vehicles  is 
incorporated  as  control  for  positioning  the  follower  vehicle. 
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I.  INTRODUCTION 


A.  BACKGROUND 

An  Autonomous  Underwater  Vehicle  (AUV)  is  an  unmanned,  self-propelled 
vehicle  that  is  able  to  provide  data  storage  as  well  as  make  navigational  and  tactical 
decisions  based  on  inputs  from  onboard  sensors.  Typically,  data  collected  from  inertial 
sensors  is  integrated  and  processed  by  an  onboard  computer  in  order  to  estimate  the 
position  and  orientation  of  the  vehicle.  This  onboard  computer  also  controls  the  vehicle; 
allowing  it  to  complete  its  mission  with  very  little  human  interaction.  AUV’s  are 
typically  battery  powered.  However,  many  different  power  sources  have  been 
experimented  with  recently  in  order  to  meet  the  needs  of  increased  mission  length 
(Miller,  2002).  Development  of  AUV’s  began  as  far  back  as  1960.  For  a  history  on 
AUV  development,  see  (Blidberg,  2001).  AUV’s  are  developed  by  many  different 
institutions;  to  include  government,  commercial,  and  educational. 

Today,  with  the  Global  War  on  Terrorism  currently  taking  place,  the  United 
States  Navy  employs  a  wide  range  of  AUV’s  to  meet  the  ever  expanding  requirements  set 
forth  by  current  and  future  strategy.  In  recent  years,  the  United  States  Navy  has  adopted 
a  strategy  that  shifts  its  priorities  from  blue  water  to  littoral  operations  and  amphibious 
support.  Sea  Power  21,  the  current  Navy  strategy,  directs  the  “use  of  unmanned 
platforms:  Air,  Land,  Sea,  and  Undersea  for  combat  and  reconnaissance  (Clarke,  2002).” 
The  Navy  has  detennined  that  there  are  nine  key  mission  areas  that  Unmanned 
Underwater  Vehicles  (UUV’s)  must  be  designed  for  in  order  to  be  successful  in  today’s 
battle  arena  as  well  as  the  battle  arena  50  years  from  now.  These  mission  areas  include: 
1)  intelligence,  surveillance,  and  reconnaissance;  2)  mine  countermeasures;  3)  anti¬ 
submarine  warfare;  4)  inspection/identification;  5)  oceanography;  6) 
communication/navigation  network  node;  7)  payload  delivery;  8)  information  operations; 
and  9)  time  critical  strikes  (UUV  Master  Plan,  2004). 
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B.  MOTIVATION 

The  primary  motivating  factor  behind  this  research  is  to  allow  formation  flying  of 
AUV’s  to  occur  without  the  use  of  an  acoustic  modem  as  the  primary  source  of 
communicating  position  between  the  vehicles.  This  will  free  up  bandwidth  exchanged 
between  acoustic  modems  located  on  each  vehicle.  This  additional  bandwidth  will  allow 
for  an  increased  efficiency  in  the  transfer  of  mission  data  between  vehicles;  which  in  turn 
will  allow  for  operators  to  receive  data  quicker  (helping  with  planning  and  executing  of 
real  world  missions). 

A  secondary  motivating  factor  behind  this  research  is  to  give  vehicles  the 
capability  to  become  more  robust  and  autonomous.  The  addition  of  forward  looking 
sonar  to  the  Remote  Environmental  Measuring  Unit  System  (REMUS)  vehicle  will  allow 
this  to  occur.  In  addition  to  multiple  vehicle  control,  forward  looking  sonar  will  allow  for 
obstacle  avoidance  (Fodrea,  2002).  This  thesis  will  present  a  solution  to  the  fonnation 
flying  problem  for  multiple  AUV’s. 


C.  FORMATION  FLYING  FOR  AUTONOMOUS  UNDERWATER 

VEHICLES 

In  order  to  stay  ahead  of  its  adversaries,  the  United  States  must  switch  from  single 
AUV  technology  to  multi-AUV  technology.  This  switch  in  design  and  mission  planning 
has  been  examined  by  many  different  institutions  in  recent  years.  Early  research  in  this 
area  had  vehicles  relying  on  beacons  or  locator  sensors  in  order  to  control  the  fonnation. 
More  recently,  “follow  the  leader”  behaviors  have  been  designed  to  rely  on  ranging  and 
intention  information  shared  between  vehicles  using  acoustic  communications  (Kucik, 
2003).  This  thesis  will  go  one  step  further  and  concentrate  on  using  a  “follow  the  leader” 
behavior  that  relies  on  ranging  and  bearing  infonnation  received  from  forward  looking 
sonar. 


D.  THE  REMUS  VEHICLE 

The  original  REMUS  vehicle  was  developed  at  Wood’s  Hole  Oceanographic 
Institute  (WHOI).  In  2001,  REMUS  entered  commercial  production  and  is  now  sold  by 
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Hydroid,  Inc.  REMUS  is  used  for  a  number  of  applications  including  harbor  security  and 
mine  countermeasure  operations. 

It  is  designed  to  perfonn  in  the  Very  Shallow  Water  (VSW)  zone  from  40  to  100 
feet.  As  seen  in  Figure  1,  it  is  a  two-man  portable  system  that  is  7.5”  (19  cm)  in 
diameter,  63”  (160  cm)  long,  and  weighs  80  pounds  (37  kg).  REMUS  can  reach  a 
maximum  speed  of  5.45  knots  (2.8  m/s)  and  a  maximum  depth  328  feet  (100  m).  Table  1 
includes  functional  and  physical  specifications  of  the  REMUS  vehicle. 


Figure  1.  REMUS  Vehicle  (from  Hydroid  Inc.,  2005) 


REMUS  can  be  configured  with  many  different  types  of  sensors,  such  as:  side 
scan  sonar,  an  acoustic  doppler  current  profiler  (ADCP),  inertial  navigation  system,  and 
acoustic  modem.  The  navigation  system  includes  a  compass,  the  above-mentioned 
ADCP  to  provide  speed  over  ground  when  ground  lock  is  available,  and  an  acoustic  long- 
baseline  (LBL)  system  to  correct  accumulated  dead  reckoning  errors.  Currently  side-scan 
sonars  are  used  to  detect  object  on  or  near  the  sea  floor.  However,  the  addition  of 
forward-looking  sonar  would  give  it  the  ability  to  detect  objects  (such  as  another  AUV  or 
uncharted  sea  mounts)  in  front  of  the  vehicle. 
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PHYSICAL/FUNCTIONAL  AREA 

CHARACTERISTIC 

Vehicle  Diameter 

19  cm 

Vehicle  Length 

160  cm 

Weight  in  Air 

37  kg  (<80  lbs.) 

Trim  Weight  in  Air 

1  kg 

Maximum  Operating  Depth 

100  meters 

Energy 

lkw-hr  internally  rechargeable 

Lithium  ion 

Endurance 

22  hours  at  optimum  speed  of 

1.5m/s  (3  knots).  8  hours  at 

2.5m/s  (5  knots) 

Propulsion 

Direct  dive  DC  brushless  motor 
to  open  three  bladed  propeller 

Velocity  Range 

0.25  to  2.8  m/s  variable  over 
range 

Control 

2  coupled  yaw  and  pitch  fins 

On/Off 

Magnetic  switch 

External  Hook-up 

Two  pin  combined  Ethernet, 
vehicle  power  and  battery 
charging;  4pin  serial  connector 

Navigation 

Long  base  line;  Ultra  short 
base  line;  Doppler  assisted 
dead  reckon;  (Optional:  GPS) 

Transponders 

20-30  kHz  operating  frequency 
range 

Tracking 

Emergency  transponder, 
mission  abort,  and  ORE 

Trackpoint  compatible 

Sensors  Doppler  Velocity  Log 

RDI  1 .2  MHz  up/down  looking 

Side  Scan  Sonar 

600  or  900  kHz  MSTL  AUV 
model 

Table  1.  REMUS  Specifications  (from  Hydroid  Inc.,  2005) 

E.  THESIS  STRUCTURE 

The  intent  of  this  research  is  to  develop  a  forward  looking  sonar  model  that 
supports  “follow  the  leader”  behaviors  of  multiple  REMUS  vehicles.  This  is  a  two  step 
process  accomplished  through  the  following:  firstly,  develop  a  robust  steering  model  of 
two  independently  controlled  REMUS  vehicles  and  have  them  navigate  a  set  of 
waypoints;  secondly,  adapt  the  steering  model  so  that  the  second  vehicle  steering  is 
dependent  and  controlled  by  the  first  vehicle’s  steering  model. 
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Chapter  II  will  focus  on  the  development  of  the  equations  of  motion  for  the 
REMUS  AUV.  Chapter  III  explains  the  theory  and  equations  used  for  triangulating 
positions  using  ranging  information. 

Chapter  IV  describes  the  steering  control  laws  associated  with  the  EOM  for  REMUS. 
Chapter  V  will  present  a  simulation  for  the  “follow  the  leader”  behavior  using  forward 
looking  sonar.  Chapter  VI  provides  thesis  conclusions  and  recommendations  for  future 
work. 
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II.  STEERING  MODEL 


A.  GENERAL 

Rigid  body  models  are  formed  in  order  to  analyze,  predict,  and  control  motion 
behavior  of  autonomous  machines  that  travel  over  land,  air,  and  undersea.  Each  type  of 
vehicle  model  differs  in  only  the  terms  of  the  forces  applied  to  produce  motion. 
However,  these  forces  are  often  controllable  and  can  thus  be  studied  from  a  prospective 
of  stabilization.  This  chapter  will  only  deal  with  the  modeling  of  underwater  vehicles. 
The  approach  taken  with  underwater  vehicles  is  that  of  a  moving  body  in  free  space 
without  constraint.  The  forces  applied  to  underwater  vehicles  include  the  following: 
inertial,  gravitational,  hydrostatic,  propulsion,  thruster,  and  hydrodynamic  lift  and  drag 
forces.  (Healey  class  notes). 

B.  EQUATIONS  OF  MOTION  IN  THE  HORIZONTAL  PLANE 

The  following  paragraphs  describe  a  simplified  development  of  the  steering 
model  used  to  control  the  REMUS  vehicle.  For  a  more  detailed  development,  see 
(Healey,  1995).  This  model  was  adapted  from  that  of  the  ARIES  AUV  (Healey  and 
Marco,  2001)  and  is  based  on  the  following  assumptions: 

•  the  vehicle  behaves  as  a  rigid  body 

•  the  earth’s  rotation  is  negligible  for  acceleration  components  of  the 
vehicle’s  center  of  mass 

•  the  primary  forces  that  act  on  the  vehicle  are  inertial  and  gravitational  in 
origin  and  are  derived  from  hydrostatic,  propulsion,  thruster,  and 
hydrodynamic  lift  and  drag  forces. 

Before  describing  the  equations  of  motion  (EOM)  that  govern  the  REMUS 
steering  model,  a  coordinate  system  for  the  vehicle  and  its  surrounding  area  must  be 
defined.  The  EOM  are  derived  using  a  Newton-Euler  approach  that  relates  the  position 
and  motions  in  the  local  plane  to  those  in  the  global  plane.  The  geometry  of  the  local  and 
global  coordinate  system  can  be  seen  in  Figure  2. 
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T  z 

Figure  2.  Local  and  Global  Coordinate  System  (from  Marco  and  Healey,  2001) 


In  order  to  convert  from  a  local  velocity  vector  [«,  v,  w] ,  where  u  is  surge,  v  is 

sway,  and  w  is  heave,  to  a  global  velocity  vector  [^X,  7,zj ,  a  transformation  matrix 

containing  ‘Euler’  angles  (<p,9,yr)  must  be  defined.  The  transformation  matrix  (T)  is 
defined  as  follows: 


T(^,^)  = 


cos  yr  cos  9, 

cos  yr  sin  9  sin  (p  -  sin^cos  (p, 
cos  yr  sin  9  cos  (p  +  siny/sin  (j). 


sin  y/  cos  9,  -  sin  9 

sin  yr  sin  9  sin  (/)  +  cos  yr  cos  (j),  cos  9  sin  (f> 
sin  yr  sin  9  cos  (p  -  cos  yr  sin  (p,  cos  9  cos  (p 


(1) 


Transformation  from  a  global  velocity  vector  to  the  local  velocity  vector  occurs  as 
follows: 


u 

~X 

V 

=  T  {(p,9,yr)* 

Y 

w 

Z 

(2) 


Transformation  from  a  local  velocity  vector  to  a  global  velocity  vector  occurs  as  follows: 


~x~ 

u 

Y 

Z 

=  T  1  ((p,  9,yr)» 

V 

w 

(3) 
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The  global  angular  velocity  vector  [ p,q,r\  can  be  transformed  into  the  rates  of  change  of 
the  ‘Euler’  angles  as  follows: 

(p  1  sin  (/>  tan  6  cos  (p  tan  9  p 

0=0  cos  (p  -sin0  q  (4) 

xj/  0  sin^/cos#  cos  0/ cos#  r 

Healey  (1995)  derives  the  equations  of  motion  for  a  six  degree  model  as: 

SURGE  EQUATION  OF  MOTION 

m  ur-vrr  +  wrq-xG(q2  +r2^)+yG(pq-f)  +  zG(pr  +  q)  +(W -B)smO  =  Xf  (5) 

SWAY  EQUATION  OF  MOTION 

m  vr  +urr-wrp  +  xG  (pq  +  r)-yG  [p2  +  r2)  +  zG  (qr-p)  -(W  -  B)cos0smtp  =  Yf  (6) 

HEAVE  EQUATION  OF  MOTION 

m  wr-urq  +  vrp  +  xG(pr-q)  +  yG(qr  +  p)-zG[p2  +  g2)J  +  (lE-f?)cos#cos0  =  Z/.  (7) 

ROLL  EQUATION  OF  MOTION 

IXP  +  (7z  -  !y  )qr  +  IXy{Pr~q)-  lyz  ~  r1 )  - 4  ( pq  +  f )  +  m  [yG  {w~u,.q  +  vrp)  (8) 

-zG  (  vr  +  urr  -  wrp)j  -  (yGW  -  yBB )  cos  9  cos (p  +  (zGW  -  zbB)  cos  0  sin  (p  =  Kf  (9) 

PITCH  EQUATION  OF  MOTION 

+  (7-  -  h  )pr~Ixy{qr  +  p)  +  Iyz  (pq-r)  +  Ixz  (p2  -  r 2 )  -  m  [xG  ( w -  urq  +  vrp)  (10) 

—zG  ( ur  -  vrr  +  wrq )]  +  (xGW  -  xbB )  cos 0 cos  (p  +  (zGW  -  zbB ) sin 0  =  M f 

YAW  EQUATION  OF  MOTION 

//  +  (/v  —  I x ) pq  —  I „  (p2  -q2)-IyZ{pr  +  q)  +  Ix:  ( qr-p)  +  m[xG  (y  +u,.r-wrp)  (11) 

-yG  (4  -vrr  +  wrq)^-(xGW -xBB)cosOsm<p-(yGW - yBB)sind  =  Nf 
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Where: 


W  =  weight 
B  =  buoyancy 

I  =  mass  moment  of  inertia  terms 

ur,  vr,  wr  =  component  velocities  for  a  body  fixed  system  with  respect  to  the  water 
p,  q,  r  =  component  angular  velocities  for  a  body  fixed  system 
xb,  yB,  zb  =  position  difference  between  geometric  center  and  center  of  buoyancy 
xg,  }’g,  zg  =  position  difference  between  geometric  center  and  center  of  gravity 
Xf,  Yf,  Zf  KF,  Mf,  Nf  =  sums  of  all  external  forces  acting  in  the  particular  body  fixed 
direction 

In  addition,  he  presents  a  simplified  version  of  these  equations  of  motion.  In  order  to 
simplify  the  initial  equations  of  motions  the  following  assumptions  were  made: 

•  the  center  of  mass  of  the  vehicle  lies  below  the  origin 

•  xg  and  yg  are  zero 

•  the  vehicle  is  symmetric  in  its  inertial  properties 

•  motions  in  the  vertical  plane  are  negligible  (i.e.,  [wr,  p,  q,  r,  Z,  </> ,  6>]=0) 

•  ur  equals  the  forward  speed,  U0 
The  simplified  equations  of  motion  are  thus: 


u,=U0 

(12) 

mvr  =  —mU0r  +  AYf(t) 

(13) 

ll 

•fs. 

(14) 

\j/  =  r 

(15) 

X  =  Ua  cos y/-vr  sin ys  +  Ua 

(16) 

Y  -  U0  sin  y/-vr  cos  y/  +  Ucy 

(17) 

C.  HYDRODYNAMIC  COEFFICIENTS 

Healey  proposes  that  due  to  symmetry  of  the  vehicle,  one  can  heuristically 
determine  that  only  a  subset  of  motions  would  affect  the  loading  in  any  particular 
direction  (Healey  class  notes)  and  uses  the  following  expressions  to  describe 
hydrodynamic  forces  of  sway  and  yaw: 
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A  Y f  =  f(vr,  dvr  /  dt,  r,  dr  /  dt,  p,  dp  /  dt,  t ) 


(18) 


AN f  =  / (p,dp / dt,vr, dvr  / dt, r, dr  / dt,t)  (19) 

Sway,  yaw,  and  roll  motions  are  coupled.  However,  roll  motion  is  often  only  coupled 
one  way  and  not  considered  when  evaluating  horizontal  plane  steering.  The 
hydrodynamic  forces  for  sway  and  yaw  are  linearized  using  Taylor  series  expansion  to 
determine  ‘hydrodynamic  coefficients.’  The  coefficients  are  dependent  on  the  shape 
characteristics  of  the  vehicle  and  have  significant  affect  on  the  natural  stability  of  the 
vehicle.  The  expression  for  the  transverse  (sway)  force  is: 

Yf  =  YvYr  +  Y,vr  +  Yr  +  Yrr  (20) 

and  the  expression  for  rotational  (yaw)  force  is: 

Nf  =  Af  vr  +  NVr vr  +N,f  +  N,.r  (2 1 ) 


This  leads  to: 


and 


Y  = 


_  dYf 


dv,_  ’ 


Y.  =- 


dN 


f  . 


dr  ’ 


Y,  = 


dY, 

v  = 


dv..  ’ 


dY, 


f  . 


dr  ’ 


N„ 


dN 


f  . 


dv 


;  n„ 


dN 


f  . 


dr 


;  n. 


dN 


/  . 


dv. 


;Nr 


dN 


f  . 


dr  ’ 


Where: 

Y.  =  coefficient  for  added  mass  in  sway 
Y.  =  coefficient  for  added  mass  in  yaw 
F  =  coefficient  of  sway  force  induced  by  side  slip 
F  =  coefficient  of  sway  force  induced  by  yaw 


(22) 


(23) 
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N-  =  coefficient  for  added  mass  moment  of  inertia  in  sway 
N.  =  coefficient  for  added  mass  moment  of  inertia  in  yaw 
Nv  =  coefficient  of  sway  moment  from  side  slip 
Nr  =  coefficient  of  sway  moment  from  yaw 

The  hydrodynamic  coefficients  for  steering  for  the  REMUS  vehicle  were  adapted 
from  thesis  work  performed  by  MIT  (Prestero,  2001)  establishing  estimates  of  all  vehicle 
coefficients.  Upon  re-calculation,  Fodrea  (2002)  adjusted  the  hydrodynamic  coefficients 
to  account  for  variation  in  experimental  data.  Table  2  lists  the  REMUS  hydrodynamic 
coefficients  for  the  steering  model  used  during  this  experiment. 


-3.55e01  kg 

a 

1.93  kg  m/rad 

E 

-6.66e01  kg/s  (Same  as  Zw) 

K 

2.2  kg  m/s  (Same  as  Zq) 

N- 

v. 

1.93  kg  m. 

N, 

-4 . 88  kg  m"/rad 

K, 

-4.47  kg  m/ s 

K 

-6.87  kg  nr/s  (Same  as  Mq) 

-3.46e01/3.5  kg  m/ s2 

Yd 

5.06e01/3.5  kg  m/s" 

Table  2.  REMUS  Hydrodynamic  Coefficients  for  Steering  (from  Fodrea,  2002) 
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The  dynamics  of  the  vehicles  are  defined  as: 


mvr  =  -mr  +  Y-vr  +  Yvvr  +  Y-r  +  Yrr  +  YsSr(t ) 

(25) 

l-J  =  N-v,.  +  Nvvr  +  N-r  +  Nrr  +  NsSr  (t) 

(26) 

K 

II 

(27) 

D.  VEHICLE  KINEMATICS 

The  kinematics  of  the  vehicle  is  described  by  Equations  (25)  and  (26).  Ucx  and 
Ucy  are  the  current  velocities  in  the  associated  direction.  The  kinematic  equations,  along 
with  the  heading  rate,  compose  the  steering  dynamics  of  REMUS  and  can  be  expressed 
as  follows: 


1 

3 

l 

-r* 

0" 

u 

~Kr 

Yr~mU0 

0" 

Vr 

1 

1 _ 

-N*r 

L~Nr 

0 

r 

= 

NVr 

K 

0 

r 

+ 

0 

0 

1 

¥_ 

0 

0 

1 

¥_ 

0 

(28) 


where  8r{t)  represents  the  control  input  for  both  rudders. 


E.  VEHICLE  DYNAMICS 

The  final  assumption  made  for  vehicle  dynamics  (Johnson,  2001)  is  that  the  cross 
coupling  terms  in  the  mass  matrix  is  zero.  Thus,  the  final  vehicle  dynamics  are  defined 
as: 


i 

l 

0 

0" 

u 

X 

Yr  -mU0 

0" 

Vr 

1 

1 _ 

0 

4-Af 

0 

r 

= 

K, 

Nr 

0 

r 

+ 

Ns 

0 

0 

1 

_¥\ 

0 

0 

1 

¥_ 

0 

Sr(t) 


(29) 
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III.  TRIANGULATION  THEORY:  GEOMETRY  AND  EQUATIONS 

A.  INTRODUCTION 

Triangulation  is  one  of  the  most  common  methods  for  determining  position.  It  is 
used  on  many  different  navigation  systems  including:  Long  Base  Line  (LBL),  Global 
Positioning  System  (GPS),  and  LORAN.  The  simplest  method  of  triangulation  is  where 
the  vehicle’s  range  from  two  known  points  is  used  to  determine  the  vehicle’s  position. 
However,  despite  its  simplicity,  there  are  still  some  ambiguities  associated  with  this 
process.  Figure  3  shows  how  an  LBL  system  is  not  able  to  determine  where  the  vehicle 
is  located  because  the  range  rings  centered  on  the  transponders  intersect  in  two  places. 


Figure  3.  LBL  System  (from  Kucik,  2003) 

Throughout  this  thesis,  it  was  assumed  that  the  leader  vehicle  had  an  advanced 
navigation  system  (to  include  GPS)  onboard  and  can  accurately  determine  its  position 
throughout  the  simulation. 
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B.  TRIANGULATION  GEOMETRY  AND  EQUATIONS 

Triangulation  geometry  can  be  used  to  determine  the  range  and  bearing  between 
the  leader  and  follower  vehicles  based  on  their  position.  Figure  4  is  a  simplified 
triangulation  computation  using  the  x  and  y  values  of  both  vehicle’s  position. 


X 


As  seen  in  Figure  4,  and  based  on  the  Law  of  Cosines,  the  following  formulas  are  defined 
for  range  and  bearing: 

range  =  J(Y-Yf)2 +(X-Xf)2  (30) 

where 

7=  the  y-position  of  the  leader  vehicle 
Y ,  =  the  y-position  of  the  follower  vehicle 

X  =  the  x-position  of  the  leader  vehicle 
X f  =  the  x-position  of  the  follower  vehicle 

and 

bearing=atan2((7  -  Yf),  ( X  -Xf))~  y/f  (3 1) 

where  y/f  =  follower  vehicle  heading 
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IV.  CONTROL  METHODS  AND  ARCHITECTURE 


A.  GENERAL  CONTROL  THEORY 

An  underwater  vehicle  operates  with  six  degrees  of  freedom  and  must  respond  to 
hydrostatic  and  hydrodynamic  forces  from  an  ever  changing  ocean  environment.  In 
addition  to  responding  to  environmental  factors,  underwater  vehicles  also  must  overcome 
the  fact  that  actuator  dynamics  are  small,  power  and  control  is  limited  to  onboard 
capacity  of  the  vehicle,  and  human  intervention  to  correct  faults  is  not  possible  during  a 
mission  (Fodrea,  2002).  In  spite  of  the  above  conditions,  feedback  control  has  been  a 
suitable  solution  used  to  provide  commands  to  actuators  that  control  and  stabilize  the 
motion  of  underwater  vehicles  (Healey  and  Marco,  2001). 

One  of  the  main  objectives  of  autonomous  vehicles  is  that  they  must  be  robust. 
Robustness  is  obtained  by  using  feedback  of  key  motion  variables  as  obtained  by  sensors 
that  drive  actuators  (Healey  class  notes).  This  in  turn  acts  an  autopilot  and  maneuvers  the 
vehicle  as  set  during  mission  pre-planning.  Marco  describes  four  different  autopilots  for 
flight  maneuvering  control.  These  consist  of  independent  diving,  steering/heading, 
altitude  above  bottom,  and  cross-track  error  controllers.  The  four  modes  are  decoupled 
and  are  based  on  sliding  mode  control  (SMC)  theory.  Sliding  mode  controls  can 
compensate  for  nonlinear  systems  (Healey,  1992).  It  is  a  robust  method  with  a  theory 
which  allows  nth  order  systems  to  be  effectively  replaced  by  a  (n-1)  order  system.  Two 
tuning  factors  are  used  in  this  model  to  include  Eta_F/ightHeading,  // ,  and 
Phi_FlightHeading,  (f> ,  as  seen  in  Appendix  A. 

B.  REMUS  CONTROL  ARCHITECTURE 

The  original  REMUS  steering  model,  used  in  this  thesis  for  the  leader  vehicle, 
was  developed  by  Fodrea  (2002).  Both  the  leader  and  follower  vehicles  are  modeled  in 
two  dimensional  space.  Due  to  this  fact,  the  steering  controller  is  the  only  autopilot 
controller  necessary  for  modeling  the  leader  vehicle.  The  leader  vehicle  is  a  second  order 
model  that  uses  r  and  y/  as  feedback. 
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In  order  to  reach  all  of  the  desired  waypoints,  line-of-sight  guidance  is  also  implemented. 
The  follower  vehicle’s  steering  controller  is  not  as  advanced  as  the  leader  vehicle.  The 
follower  vehicles  position  is  maintained  by  control  laws  incorporating  range  and  bearing 
to  the  leader  vehicle  that  is  received  from  forward  looking  sonar. 


C.  SLIDING  MODE  CONTROL 

A  multivariable  sliding  mode  controller  is  used  to  provide  an  accurate  steering 
model.  A  multivariable  controller  is  used  with  predominantly  linear  systems  as  opposed 
to  the  SMC  methods  used  for  nonlinear  systems  (Healey,  1992).  To  create  the  SMC,  the 
general  form  of  the  equations  of  motion  is  used: 

x-  Ax  +  Bit  (32) 

where  x  e  9T'*1 ,  A  e  91"*" ,  B  e  ,  u  e  9T *' ,  and  u  is  the  rudder  angle.  The  goal  of  a 
sliding  surface  it  do  drive  the  state  to  a  stable  solution  (<7  =  0,  ere  R/,s| ).  The  sliding 
surface  is  defined  as: 


<r  =  s'x;  x  =  x-xcom  (33) 

where  s’  is  a  vector  of  directions  in  the  state  error  space.  The  elements  of  o  are  the 
lengths  of  the  projections  of  the  state  error  vector.  xcom  is  a  variable  created  as  a 
command  signal  to  track,  x  is  the  state  error  which  is  required  to  be  driven  to  zero  so  the 
command  state  equals  the  actual  state.  By  definition  of  the  sliding  mode  controller,  the 
system  dynamics  must  exhibit  stable  sliding  on  the  surface  when  a  =  0.  Therefore,  s’  can 
be  detennined  by  observing  that  the  closed  loop  dynamics  are  given  by  the  poles  of  the 
closed  loop  matrix  as, 

(. A-bk2)  =  Ac ,  with  k2=[s'B\ls'A  (34) 

where  ka  is  chosen  by  pole  placement  and  Acs’  =  0  to  achieve  the  condition  o  =  0.  The 
eigenvectors  of  the  Ac  matrix  determine  the  linear  state  feedback  gains  for  each  state  to 
define  the  sliding  surface  as  follows: 
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<7(0  =  U  (rcom  ~  r(0)  +  *3 YlOS  (0 


(35) 


The  poles  selected  for  the  REMUS  model  of  the  leader  vehicle  were  similar  to  those 
selected  by  Fodrea  (2002)  in  here  simulation.  As  seen  in  Appendix  A,  the  poles  are 
places  at  [-1.4  -1.45  0.0].  A  pole  must  be  placed  at  the  origin  in  order  to  represent  the 
single  sliding  constraint  for  the  single  input  system.  The  remaining  poles  are  in  the  open 
left  hand  plane;  which  is  a  requirement  for  stable  dynamics.  The  gains  obtained  from  this 
pole  placement,  using  the  MATLAB  command  “place”,  where  [kl  k2  k3]  =  [0.769  -0.6 
0.0]  for  [  v,r,y/\  respectively.  Using  the  gains  determined  from  pole  placement  and  the 
sliding  surface  defined  in  Equation(34),  the  commanded  rudder  in  the  LOS  controller  for 
the  leader  vehicle  becomes: 


dr(t )  =  -kl  *  r(t )  *  rj  *  tanh 


V  0  V 


where  Tj  and  (f>  are  tuning  factors  equal  to  0.5  and  0.1. 


(36) 


D.  LINE  OF  SIGHT  GUIDANCE 

A  Line  of  Sight  (LOS)  controller  is  used  on  the  leader  vehicle.  The  purpose 
behind  its  use  it  reduce  the  heading  error  to  zero.  Once  again,  this  controller  was 
developed  for  the  REMUS  vehicle  by  Fodrea  (2002).  It  uses  a  follow-the-rabbit 
technique  which  is  similar  in  nature  to  the  transducer  based  dead-reckoning  approach 
with  which  REMUS  operates  with  in  the  real  world.  This  controller  directs  the  vehicle 
towards  the  current  waypoint.  It  achieves  this  by  determining  the  heading  error ,y/Ws 
which  is  defined  as: 

¥(t)Los=¥(t)lmck-¥(t)  (37) 

where 

W)track  =  arctan(  Y(t)  wp[(i)  ,X(t)  wpt(i) )  (38) 

Figure  3  is  a  graphical  representation  of  how  the  REMUS  vehicle  model  incorporates  an 

additional  dead  reckoning  on  the  track  towards  the  next  waypoint.  The  distance  to  this 

point  is  incorporated  into  the  heading  error  as  follows: 
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W(t)ws  =  ¥if)  track 


y/{t)  -  arctan 


/  cte{t )  ^ 
K  rabbit  y 


where  rabbit  is  the  look  ahead  point  and  cte  is  the  cross  track  error. 


(39) 


Figure  4.  Track  Geometry  and  Velocity  Vector  Diagram  (from  Fodrea,  2002) 

As  the  leader  vehicle  approaches  the  waypoint,  it  must  determine  when  to  turn.  In 
order  to  do  this,  REMUS  will  begin  tracking  the  next  waypoint  when  approaching  the 
present  waypoint  based  on  the  following  formula: 

sqrt(X  _  Way  _  Error(t)2  +  Y  _  Way  _  Error (t)2  <=  W  _  R  |<S(0  <  0.0|  ss(t)  <  rabbit  (40) 

where  W_R  is  the  watch  radius  around  the  waypoint,  s  is  the  distance  remaining  on  the 
track,  and  ss  is  the  radial  distance  to  go  to  the  next  waypoint. 
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E.  CONTROL  LAWS  LOR  LOLLOWER  VEHICLE 

One  of  the  primary  objectives  of  this  thesis  was  to  design  a  set  of  control  laws  that 
would  position  the  follower  vehicle.  As  seen  above  with  the  leader  vehicle,  the  control 
laws  for  the  follower  vehicle  were  designed  based  on  sliding  mode  control.  Control  laws 
for  the  follower  vehicle  were  based  on  incorporating  range  and  bearing  to  the  leader 
vehicle  that  was  received  from  forward  looking  sonar. 

Based  on  the  range  between  the  leader  and  the  follower  vehicle,  the  follower 
vehicle  would  adjust  its  speed  (U)  in  order  to  achieve  the  command  range  (RCOm).  Range 
error  is  defined  as: 

R  =  (R(t)-Rcom)  (41) 

In  order  to  determine  the  speed  of  the  follower  vehicle,  the  following  fonnulas  are 
defined: 

&R  =-tj  sgn(aR)  (42) 

where  oR  =  R  .  Also, 


&r=R  =  (R- 0) 


R  is  defined  as: 


R  = 


R 


{x0~x)  + 


yQ-y 

R 


J 


{y0~y) 


where 

xa  =  leader  vehicle’s  x-position 
x  =  follower  vehicle’s  x-position 
=  U0  cos(^0) 
x  =  U  cos  (ys) 

ya  =  leader  vehicle’s  y-position 


(43) 


(44) 
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y  =  follower  vehicle’s  y-position 


yo  =  U0  sin{(//() 
y  =  U  sin(^) 

Combining  Equations  (42)  and  (44)  and  solving  for  U  results  in  the  following  fonnula: 

U  =  ~  sgn  (R)R  +  x0  (x-x0)  +  yo  (y  -  y0 )  ^ 

cos  y/(x  -  x0 )  +  sin  yr(y  -  yo ) 

Based  on  the  bearing  between  the  leader  and  the  follower  vehicle,  the  follower 
vehicle  would  adjust  its  heading  ( y/f )  in  order  to  achieve  the  command  bearing  ( /3rom  ). 

Bearing  error  is  defined  as: 

=  (46) 

In  order  to  determine  the  heading  of  the  follower  vehicle  the  following  formulas  are 
defined: 

& p  =-^7sgn(^  +  /l^)  (47) 

Sr  =  -T]  sgn(^  +  Xr)  (48) 

After  a  trial  and  error  period,  the  following  formula  was  produced  in  order  to  provide 
stable  results. 

8r=-5*P  +  \0*r  (49) 

The  formulas  listed  throughout  this  chapter  are  incorporated  into  the  model  as 
seen  in  Appendix  A.  Results  based  on  these  formulas  are  discussed  later  in  this  thesis 
(Chapter  V). 
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V.  VEHICLE  SIMULATION 


A.  INTRODUCTION 

A  MATLAB  script  file  (Appendix  A)  was  written  in  order  to  simulate  multi- 
vehicle  operations.  This  script  file  contains  control  laws  and  cooperative  behaviors  that 
support  and  evaluate  the  “follow  the  leader”  behavior  presented  in  Chapter  IV.  The 
following  settings  were  incorporated  into  the  simulation: 

•  The  minimum  distance  between  the  leader  vehicle  and  the  follower 
vehicle  (Rcom)  was  set  to  10  meters. 

•  The  leader’s  vehicle  speed  is  set  to  1.543  m/s. 

•  The  follower’s  minimum  and  maximum  speed  is  set  to  1  m/s  and  2  m/s, 
respectively. 

B.  REMUS  SEARCH  PATH 

Due  to  the  autonomy  of  the  vehicle,  REMUS  is  pre-programmed  with  a  planned 
path  of  travel  before  the  mission  begins.  The  search  path  is  commonly  referred  to  as  the 
lawnmower  technique  and  is  used  to  cover  a  square  grid  area.  This  thesis  models  a 
REMUS  path  the  uses  rows  approximately  150  meters  in  length  with  100  meters  of 
separation  as  seen  in  Figure  5. 
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Figure  5.  Typical  REMUS  Search  Path 
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C.  SONAR  MODEL 

This  model  uses  a  two  dimensional  forward  looking  sonar  with  a  120°  horizontal 
scan  and  a  1 10  meter  radial  range  as  seen  in  Figure  6.  This  is  an  estimated  range  based 
on  a  viable  400KHz  sonar  frequency.  The  use  of  forward  looking  sonar  would  alleviate 
exchanging  position  information  via  acoustic  modem.  Resulting  bearing  and  range  is 
used  to  control  position  of  the  follower  vehicle. 


Figure  6.  Forward  Looking  Sonar  Model  (from  Fodrea,  2002) 

D.  MULTIPLE  VEHICLE  SIMULATION  (NO  CURRENT) 

The  first  objective  of  this  simulation  was  to  have  the  leader  and  follower  vehicle 
navigate  a  set  of  7  waypoints  depicting  a  typical  “real  world”  search  path.  At  this  point, 
current  was  not  injected  into  the  simulation  model.  Figure  7  depicts  the  tracks  for  the 
leader  and  follower  vehicles  during  the  mission  simulation. 
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Figure  8  shows  an  initial  offset  of  the  follower  vehicle.  This  offset  was 
performed  in  order  to  show  the  initial  rendezvous  between  the  leader  and  follower 
vehicles. 
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Figure  8.  Initial  Rendezvous  Between  Vehicles 


Figures  9  and  10  show  the  range  and  bearing  between  the  leader  and  the  follower 
vehicle.  Range  and  Bearing  were  the  main  variables  in  controlling  the  position  of  the 
follower  vehicle.  The  command  range  was  set  to  10  meters  and  the  command  bearing 
was  set  to  0  degrees  for  this  simulation. 

From  Figure  9,  it  is  seen  that  initial  range  is  approximately  35  meters.  This  is  due 
to  the  initial  offset.  The  range  then  decreases  down  to  10  meters  as  expected.  Also,  the 
range  drops  below  10  meters  at  5  different  locations  during  the  simulation.  This  drop  in 
range  corresponds  to  the  leader  vehicle  turning  to  the  next  waypoint. 


25 


40 

35' 

30 

£  25 

CD 

o 

CTJ 

.1  20 
o 

15 

10 

Range  Between  Leader  and  Follower  Vehicle 

\ 

V 

\ 

_ 

_ 

_ 

_ 

w0  1000  2000  3000  4000  5000  6000  7000  8000  9000 
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From  Figure  10,  it  is  seen  that  initial  bearing  is  approximately  -45  degrees.  As 
stated  above,  this  is  due  to  the  initial  offset  of  the  follower  vehicle.  The  bearing  then 
decreases  down  to  0  degrees  as  expected.  Bearing  between  the  two  vehicles  changes 
during  the  turns  as  did  the  range 


Figure  10.  Bearing  Between  Leader  and  Follower  Vehicle 
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Figures  11  and  12  contain  a  “zoomed  in”  look  at  the  mission  simulation. 
Specifically,  Figure  1 1  is  a  “zoomed  in”  look  at  the  vehicles  while  they  are  traveling  in  a 
straight  line.  Figure  12  is  a  “zoomed  in”  look  at  the  vehicles  during  a  turn. 


Multi-Vehicle  Mission  Run 


Figure  1 1 .  Zoomed  In  Look  (Straight  Path) 
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Figure  12.  Zoomed  In  Look  (Turn) 
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E.  MULTIPLE  VEHICLE  SIMULATION  (WITH  CURRENT) 

The  next  step  of  this  thesis  was  to  add  the  affects  of  current  in  to  the  simulation. 
Current  was  added  to  achieve  a  more  accurate  simulation  of  a  real  world  mission.  Figure 
13  depicts  the  tracks  for  the  leader  and  follower  vehicles  during  the  mission  simulation 
containing  the  affects  of  current.  Looking  at  Figure  9,  the  follower  vehicle  does  not 
appear  to  be  directly  behind  the  leader  vehicle  as  seen  in  the  earlier  simulation. 


Multi-Vehicle  Mission  Run 


Figure  13.  Leader  and  Follower  Vehicle  Path  (affected  by  currents) 


Figures  14  and  15  contain  a  “zoomed  in”  look  at  the  mission  simulation. 
Specifically,  Figure  14  is  a  “zoomed  in”  look  at  the  vehicles  while  they  are  traveling  in  a 
straight  line.  Figure  1 5  is  a  “zoomed  in”  look  at  the  vehicles  during  a  turn. 

From  Figure  13,  it  initially  looks  like  the  follower  vehicle  is  offset  in  comparison 
to  the  leader  vehicle.  However,  the  leader  vehicle  actually  has  a  range  of  10  meters  and  a 
bearing  of  0  degrees.  This  is  shown  in  Figures  16  and  17.  Both  the  leader  and  follower 
vehicles  are  “crabbed”  in  order  to  offset  the  current  and  reach  the  desired  waypoints. 
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Multi-Vehicle  Mission  Run 


Figure  14.  Zoomed  In  Look  (Straight  Path  w/  Currents) 


Multi-Vehicle  Mission  Run 


Figure  15.  Zoomed  In  Look  (Turn  w/  Currents) 


29 


Range  Between  Leader  and  Follower  Vehicle 

35i 

30 

£  25 

o 

c 

03 

20 

o 

15 

10 

_ 

w 

_ 

"0  1000  2000  3000  4000  5000  6000  7000  8000  9000  10000 

Figure  16.  Range  Between  Leader  and  Follower  Vehicle  (w/  currents) 


Figure  17.  Bearing  Between  Leader  and  Follower  Vehicle  (w/  currents) 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

This  thesis  modeled  and  simulated  formation  flying  of  multiple  AUV’s  using 
forward  looking  sonar  as  a  method  of  controlling  the  position  of  the  follower  vehicle  in 
the  fonnation.  The  research  completed  by  this  thesis  provides  the  tools  necessary  to 
develop  more  sophisticated  cooperative  behaviors  (i.e.,  multiple  vehicle  obstacle 
avoidance).  The  proposed  control  laws  presented  in  this  thesis  successfully  operates  in 
the  simulation  enviromnent.  The  follower  vehicle  adjusted  its  speed  and  heading 
accordingly  to  obtain  the  commanded  position  accurately. 


B.  RECOMMENDATIONS 

There  are  many  areas  in  which  this  thesis  work  can  be  improved  upon  to  build  a 
more  complete  and  robust  formation  model  for  the  REMUS  vehicle.  One  such  area  is  the 
development  of  a  three  dimensional  model.  This  would  require  the  addition  of  depth  to 
the  sonar  scan.  This  in  turn  would  require  the  modification  of  the  vehicle  equations  of 
motion  to  include  diving  and  climbing  maneuvers. 

Another  area  of  future  work  is  to  implement  obstacle  avoidance  algorithms  into 
the  simulation.  Obstacle  avoidance  algorithms  have  been  designed  for  use  with  REMUS 
vehicles  using  forward  looking  sonar.  However,  it  has  yet  to  be  expanded  to  multiple 
vehicle  formations. 

Lastly,  upon  completion  of  simulation  modeling,  real  world  experimentation  must 
take  place  in  order  to  validate  results  received  form  various  simulations  using  forward 
looking  sonar.  After  initial  experimental  data  has  been  collected,  the  simulation  model 
may  need  to  be  refined  to  account  for  environmental  factors  not  originally  foreseen  by  the 
model. 
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APPENDIX 


%  Multi  -  Vehicle  (Dependent  Control) 

%  This  m-file  simulates  a  multi-vehicle  follow  the  leader  formation. 

%  It  uses  corrected  hydrodynamic  coeff  from  MIT  to  develop  a  steering 
%  model  for  the  leader  vehicle.  The  follower  vehicle  is  positioned 
%  based  on  bearing  and  range  to  the  leader  vehicle  received  from  a 
%  forward  looking  sonar  that  is  attached  to  the  follower  vehicle. 

clear, close  all; 
clc 


DegRad  =  pi/180; 
RadDeg  =  180/pi; 


%  REMUS  Characteristic 
L  =  1.33;  % 

W  =  2 . 99e02  ;  % 

g  =  9.81;  % 

m  =  W/g;  % 

V  =  1.543;  % 

rho  =  1 . 0  3  e  0  3  ;  % 

D  =  .191;  % 


Specifications 
Length  in  m 
Weight  in  N 

Acceleration  of  gravity  in  m/s 

Mass  in  kg 

Max  Speed  in  m/ s 

Density  of  Salt  H20  in  kg/mA3 

Max  diameter  in  m 


a2 


%  State  Model  Parameters 

Uo  =  1.543;  %  in  m/s 

Boy  =  2 . 99e02 ; 

xg  =  0;  yg  =  0;  zg  =  1.96e-02;  %  in  m 


Iy  =  3.45;  %  kg/mA3  (from  MIT  thesis) 

Iz=Iy ; 


%  MIT  REMUS  Coeff  (Dimensionalized) 

Nvdot  =  1.93; 

Nrdot  =  -4.88; 

Yvdot  =  -3 . 55e01 ; 

Yrdot  =  1.93; 

%  Nv  =  -4.47;  should  be  same  as  Mw  which  is  stated  as  +30.7 
%  should  be  -9.3  but  going  by  Hoerner  eqn,  we  get  about  4.47 
Nv  =  -4 .47; 

Nr  =  -6.87;  %  Same  as  Mq; 

Yv  =  -6 . 6 6 e 0 1 ; 

%  Same  as  Zw;  Note  should  be  -6.66el  from  MIT  thesis  not  2.86e01 
Yr  =  2.2  ;  %  Same  as  Zq  =  2.2;  MIT  has  miscalculation 

Nd  =  -3 . 46e01/3 . 5 ;  %  Nd  and  Yd  scaled  by  3.5  to  align  w/exp  data 

Yd  =  5.06e0l/3.5; 

%  REMUS  Steering  Equations 

MM= [ (m-Yvdot )  -Yrdot  0;-Nvdot  (Iz-Nrdot)  0;0  0  1] ; 

AA= [Yv  (Yr-m*V)  0;Nv  Nr  0;  010]; 

BB=  [Yd ; Nd ; 0]  ; 

A=inv (MM) *AA;  B=inv (MM) *BB ;  C= [0,0,1];  D=0; 
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A2  = [A ( 1 : 2 , 1 ) , A (1:2, 2) ] ;B2= [B(l) ;B(2) ] ; 
xss=inv (A2 ) *B2 ; 
poles  =  eig(A2); 

RadGy  =  sqrt (Iz/ (W/g) ) ; 

RadCurv  =  Uo/(xss(l)); 

Sideslip  =  atan2 (xss ( 1 ) , Uo) *180/pi ; 


%  in  m 
%  in  m 
%  in  deg/s 


[num, den] =ss2tf (A, B, C, D) ;  z=roots (num) ;  p=roots (den) ; 


%  Desired  closed  loop  poles  for  sliding 
k=place (A, B ,  [-1.4, -1.45, 0.0]); 


%  Closed  loop  dynamics  matrix 
Ac  = A - B  *  k ; 

[m, n] =eig (Ac  1 ) ; 

S=m ( : , 3 ) ; 


Define  Sonar  Grid  Parameters: 
sonrange  =  110; 

o 

o 

radial  range  in  m  based  on  FLS  chart 

theta  =  2*pi/3; 

o 

o 

angular  arc  in  rad 

dtheta  =  l*pi/180; 

o, 

o 

angular  bin  length  in  rad 

thetab  =  theta/dtheta; 

o 

o 

number  of  bins  angularly 

offset  =  0; 

%  Set  time  of  run 
dt  =  0.125/2; 
t  =  [0 :dt : 1800]  '  ; 

size  (t)  ; 

%  Set  initial  conditions 
start=10 ; 
v ( 1 )  =  0.0; 

r ( 1 )  =  0.0; 

rRM ( 1 )  =  r  ( 1 )  ; 

r_com  =  zeros (1, length (t) -1) ; 

vf ( 1 )  =  0.0; 

rf ( 1 )  =  0.0; 

rRMf  (1)  =  rf (1)  ; 

r_comf  =  zeros (1, length (t) -1) ; 

%  This  is  the  initial  heading  of  the  vehicle 
psi(l)  =  0.0*DegRad; 

psif (1)  =  0.0*DegRad; 

%  This  is  the  initial  position  of  the  vehicle 
X ( 1 )  =  0.0;  %  in  m 
Y ( 1 )  =  100.0; 

Xf (1)  =  -25 .0;  %  in  m 
Yf ( 1 )  =  125.0; 
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%  This  data  from  track. out  file 
No_tracks=7 ; 


[100 . 0 

100 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

8 . 00 

40 . 00 

200 . 0 

100 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

8 . 00 

200 . 00 

200 . 0 

250 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

2 . 00 

15 . 00 

300.0 

250 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

2 . 00 

200 . 00 

300.0 

100 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

2 . 00 

200 . 00 

400 . 0 

100 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

2 . 00 

200 . 00 

400 . 0 

250 . 0 

2 . 75 

2 . 75 

0 

1 . 25 

1 . 00 

0 

25 . 00 

2 . 00 

200 . 00] 

track=Track ( : , 1 : 2 ) ; 
SurfaceTime  =  Track(:,9); 
Surf Phase  =  Track(:,8); 


%  Read  in  waypoints  from  track  data  assumes  track  is  loaded 
for  j=l :No_tracks, 

X_Way_c(j)  =  track ( j, 1); 

Y_Way_c ( j )  =  track ( j , 2 ) ; 

end; 


PrevX_Way_c ( 1 )  =  X ( 1 ) ; 

PrevY_Way_c ( 1 )  =  Y ( 1 ) ; 


%  Set  rudder  angle  saturation: 
sat  =  9;  %  Degrees 


%  Set  Watch  Radius : 

W_R  =  2.0; 

%  Set  dead-reckoning/look-ahead  distance: 
rabbit  =  7; 


x ( : , 1 )  =  [v (1)  ; r (1)  ;psi  (1)  ]  ; 
xf ( : , 1 )  =  [vf  (1)  ; rf (1)  ;psif  (1) ]  ; 

Eta_FlightHeading  =0.5;  %  Lowered  this  from  1.0  on  ARIES  model 

Phi_FlightHeading  =0.1;  %  Lowered  this  from  0.5  on  ARIES  model 

%  Below  for  tanh 
Eta_CTE  =  0.05; 

Eta_CTE_Min  =  1.0; 

Phi_CTE  =  0.2; 

Uc  =  0.0; 

VC  =  0.0; 

SegLen(l)  =  sqrt ( (X_Way_c (1) -PrevX_Way_c (1) ) ^2+ (Y_Way_c (1) - 
PrevY_Way_c (1) ) A2) ; 

psi_track(l)  =  atan2 (Y_Way_c (1) -PrevY_Way_c (1) , X_Way_c (1) - 
PrevX_Way_c ( 1 ) ) ; 
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for  j  =2 : No_tracks , 

SegLen ( j )  =  sqrt ( (X_Way_c ( j ) -X_Way_c ( j -1) ) A2  + (Y_Way_c ( j ) -Y_Way_c ( j - 
1) ) A2)  ; 

psi_track ( j )  =  atan2 (Y_Way_c ( j ) -Y_Way_c ( j -1) ,X_Way_c ( j ) -X_Way_c ( j - 
1)  )  ; 
end; 


j=l; 

Sigma  =  []  ; 

Depth_com  =  [] ; 
dr=  []  ; 
drl  =  []  ; 
drl(l)  =  0.0; 
psint=  []  ; 

Depth_com ( 1 )  =  5.0; 

WayPointVertDist_com  =  [5.0  5.0  5.0  5.0  5.0  5.0  5.0]; 

%  Initialize  path  planning 

st art = zeros (1 , length (t ) -1) ; of f set =s tart ; 

Xdev=start ; Ydev=start ; 
count =0 ; 
inplan=0 ; 
psiapf =start ; 

%  Begin  mission  simulation 
for  i=l : length (t )- 1 

Depth_com(i)  =  WayPointVertDist_com(j); 

X_Way_Error ( i )  =  X_Way_c(j)  -  X(i); 

Y_Way_Error ( i )  =  Y_Way_c(j)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi; 
psi_cont(i)  =  psi(i); 

while (abs (psi_cont (i) )  >  2.0*pi) 

psi_cont(i)  =  psi_cont(i)  -  sign (psi_cont ( i ) ) *2 . 0*pi ; 
end; 

%  DeWrap  psi_error  to  within  +/-  pi; 
psi_errorCTE (i)  =  psi_cont(i)  -  psi_track(j); 

while (abs (psi_errorCTE (i) )  >  pi) 

psi_errorCTE (i)  =  psi_errorCTE ( i )  -  sign (psi_errorCTE ( i ) ) *2 . 0*pi 

end; 


%  **Always  calculate  this** 

Beta  =  0.0; 

cpsi_e  =  cos (psi_errorCTE (i) +Beta) ; 
spsi_e  =  sin (psi_errorCTE (i) +Beta) ; 
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s ( i )  =  [X_Way_Error ( i ) , Y_Way_Error ( i ) ] * . . . 

[ (X_Way_c ( j ) -PrevX_Way_c ( j ) )  ,  (Y_Way_c ( j ) -PrevY_Way_c ( j ) ) ]  1 

%  s  is  distance  to  go  projected  to  track  line (goes  from  0-100%L) 
s (i)  =  s (i) /SegLen ( j ) ; 

Ratio= (1 . 0-s (i) / SegLen ( j ) ) *100 . 0 ; 

%  ss  is  the  radial  distance  to  go  to  next  WP 
ss(i)  =  sqrt (X_Way_Error ( i ) ^2  +  Y_Way_Error ( i ) ^2 ) ; 

%  dp  is  the  angle  between  line  of  sight  and  current  track  line 
dp  ( i )  =  ... 

atan2 (  (Y_Way_c ( j ) -PrevY_Way_c ( j ) ) , (X_Way_c ( j )  - 
PrevX_Way_c ( j ) )  ) . . . 

-  atan2 (  Y_Way_Error (i) , X_Way_Error (i)  ); 

if (dp (i)  >  pi) , 

dp ( i )  =  dp(i)  -  2 . 0*pi ; 
end; 


cte(i)  =  s (i) *sin (dp (i) ) ; 

if (  abs (psi_errorCTE (i) )  >=  00 . 0*pi/180 . 0) 

%  Use  LOS  Control 

LOS ( i )  =  1 ; 

psi_comLOS (i)  =  atan2 (Y_Way_Error ( i ) , X_Way_Error ( i ) ) ; 
cc=0  ; 

psioalook (i) =0 ; 
r_com (i) =0.0; 

if  cc>0 

psioatot (i) =psioalook (i) /cc; 

else 

psioatot (i) =psioalook (i) ; 

end; 

%  Integral  control  -  limit  error  only  activate  if  i>2 
psint (1) =0 ; 
ctel (i) =cte  (i)  ; 

if  (abs (cte (i) ) >  2),  ctel (i) =2*sign (cte (i) ) ;end; 

if  ( i >1 )  ,  psint  (i)  =psint  (i-1) +dt*cte  (i)  ,-end; 

psi_errorLOS (i)  =  psi_comLOS ( i )  -  psi_cont(i)- 
(atan2 (cte (i) , rabbit) ) *1 . 0  -0 . 0*psint (i) + 

0 . 0*psioatot (i) +1 . 0*psiapf (i) ; 
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if (abs (psi_errorLOS (i) )  >  pi), 

psi_errorLOS (i)  =  ... 
psi_errorLOS (i)  - 

2 . 0*pi*psi_errorLOS (i) /abs (psi_errorLOS (i) ) ; 
end; 

Sigma_FlightHeading (i)  =  ( -S (1 , 1) *v (i) ) *0 . 0  +  S (2 , 1) * (r_com (i)  - 

r(i))  +  S (3 , 1) *psi_errorLOS (i) ; 

%  Have  taken  out  v  influence  in  Sigma_FlightHeading  above 

dr(i)  =  ( -k (1 , 1) *v (i) ) *0 . 0-k (1 , 2 ) *r (i)  ... 

Eta_FlightHeading*tanh (Sigma_FlightHeading (i) /Phi_FlightHeading) ; 
else 

%  Use  CTE  Controller 
LOS ( i )  =  0 ; 

if (cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  SMC  Soln 

Sigma(i)  =  Uo*rRM ( i ) *cpsi_e  +  Laml*Uo*spsi_e  +  Lam2*cte(i); 

dr(i)  =  (1 . 0/ (Uo*a*cpsi_e) ) * (-Uo*b*rRM (i) *cpsi_e  + 

Uo*rRM (i) ^2*spsi_e  ... 

-  Laml*Uo*rRM (i) *cpsi_e  -  Lam2*Uo*spsi_e  - 
2 . 0*Eta_CTE* (Sigma (i) /Phi_CTE) ) ; 

else 

dr ( i )  =  dr ( i - 1 ) ; 
end; 

end;%  End  of  CTE  Controller 

if (abs (dr ( i ) )  >  sat*pi/180)  %  change  from  0.4  radians  on  ARIES 

dr(i)  =  sat*pi/180*sign (dr (i) ) ; end; 

%Range  and  Bearing 
Rcom  =10;  %in  m 

range  (i)  =sqrt  (  (Y  (i)  -  Yf  (i)  )  ^2  +  (X  ( i )  -Xf  (i)  )  ^2)  ; 

R_err ( i ) =range ( i ) -Rcom; 

U(i) = (-sign (R_err (i) ) * range (i) +Uo*cos (psi (i) ) * (Xf (i) - 
X (i) ) +Uo*sin (psi (i) ) * (Yf (i) -Y (i) ) ) / (cos (psi (i) ) * (Xf (i) - 
X  (i) ) +sin (psi (i) ) * (Yf (i) -Y(i) ) ) ; 

Bcom  =  0 ; 

bearing (i) =atan2 ( (Y(i) -Yf (i) )  ,  ( X ( i ) -Xf (i) ) ) -psif (i)  ; 
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B_err (i) =bearing (i) -Bcom; 


if  (abs (B_err (i) ) >pi) ,  B_err (i) =B_err (i) -2*pi*sign (B_err (i) ) ;end; 


drf (i) =-5*B_err (i)+10*rf(i)  ; 
maxspeed  =  2 ; 
minspeed  =  1  ; 

if  U (i) cminspeed 
U (i) =minspeed; 

end; 

if  U (i) >maxspeed 
U (i) =maxspeed; 

end; 


%  State  Variable  Formulation 

x_dot(:,i+l)  =  [  A(l,l)*v(i)  +  A(l,2)*r(i)  +  B(l)*dr(i); 

A (2 , 1) *v (i)  +  A (2 , 2 ) *r ( i)  +  B(2)*dr(i); 
r  ( i )  ]  ; 

x ( :  ,  i  +  1 )  =  x (  :  ,  i) +dt*x_dot ( : , i)  ; 
v (i  +  1)  =  X (1 , i  +  1)  ; 

r (i  +  1)  =  x (2  ,  i  +  1)  ; 

psi(i  +  l)  =  x ( 3 , i  + 1 )  ; 
rRM (i  +  1)  =  r (i  +  1)  ; 

x_dotf ( : , i+1)  =  [  A(l,l)*vf(i)  +  A(l,2)*rf(i)  +  B(l)*drf(i); 

A(2,l)*vf(i)  +  A(2,2)*rf(i)  +  B(2)*drf(i); 
rf  (i) ]  ; 

xf  (  :  , i  +  1)  =  xf ( :  , i) +dt*x_dotf ( :  , i)  ; 

vf(i+l)  =  xf ( 1 , i+1 ) ; 

rf(i  +  l)  =  xf ( 2 , i  +  1 )  ; 

psif  (i  +  1)  =  xf ( 3 , i  +  1 )  ; 

rRMf (i  +  1)  =  rf (i  +  1)  ; 

%  Kinematics 

X(i+1)  =  X(i)  +  (Uc  +  (Uo) *cos (psi (i) )  -  v ( i ) *sin (psi ( i) )  )*dt; 

Y(i+1)  =  Y(i)  +  (Vc  +  (Uo) *sin (psi (i) )  +  v (i) *cos (psi (i) )  )*dt; 

Xf (i+1)  =  Xf (i)  +  (Uc  +  (u(i) ) *cos (psif (i) )  -  vf (i) *sin (psif (i) )  )*dt; 
Yf (i+1)  =  Yf (i)  +  (Vc  +  (U (i) ) *sin (psif (i) )  +  vf (i) *cos (psif (i) )  )*dt; 

%  Check  to  see  if  we  are  within  the  WR  or  if  we  passed  the  WP 
%  Change  to  next  WP  if  radial  distance  to  go  is  less  than  rabbit 
%  distance  or  if  we  passed  the  WP  or  if  we  are  within  the  WR 

if (sqrt (X_Way_Error (i) ^2 . 0  +  Y_Way_Error ( i ) ^2 . 0 )  <=  W_R  |  s(i)  < 

0 . 0 |  ss (i) crabbit  ) 

disp (sprintf (' WayPoint  %d  Reached  1 , j ))  ; 
if ( j==No_tracks) , 

%  PLOT_PART  =  1; 
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break; 

end; 

PrevX_Way_c ( j+1)  =  X_Way_c(j); 
PrevY_Way_c ( j+1)  =  Y_Way_c(j); 

j=j+i; 

end; 


end; 


dr (i+1)  =  dr (i) ; 
cte (i  +  1)  =  cte  (i)  ; 
s (i  +  1)  =  s (i)  ; 
ss (i  +  1)  =  ss (i)  ; 
range (i+1) =range (i) ; 
bearing (i+1) =bearing (i) ; 

R_err ( i+1 ) =R_err (i) ; 

U  ( i  + 1 ) =U ( i )  ; 

figure  (1)  ; 

for  i=l : 100 : length (Y) 

plot (Y ( i ) , X ( i ) , 1 r . ' ) ;  title (' Multi -Vehicle  Mission  Run');  xlabel('Y 
(m)  ' )  ,  ylabel ( 1 X  (m)  1 )  ;  legend ( 1  Leader  Vehicle  1 ,  1  Follower  Vehicle ' )  ; 
axis ( [0  300  -50  450] ) ; 
hold  on 

plot (Yf (i) ,Xf (i) , 'g* ' ) ; 
axis (  [0  300  -50  450]  )  ; 
grid  on;  hold  on, 
end; 


figure (2); 

plot (range, 1 r .') ;  title('Range  Between  Leader  and  Follower  Vehicle'); 
ylabel ( ' Distance  (m) 1 ) ; 
grid  on; 

figure (3); 

plot (bearing . *180/pi , 1 b . 1 ) ;  title (' Bearing  Between  Leader  and  Follower 
Vehicle' ) ;  ylabel ( 'Bearing  (deg) ' ) ; 
grid  on; 
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